/* EasyCODE V8 */
/* EasyCODE ( 0 
PIC32Flight2.0 */
/* EasyCODE ( 0 
Definitions */
/* EasyCODE ( 0 
Define */
#include <xc.h>
#include <plib.h>
#include <string.h>
#include <math.h>
#include "JUART.h"
#include "JI2C.h"
#include "MPU.h"
#include "HMC5883L.h"
#include "MS5611.h"

// PIC32MX795F512H Configuration Bit Settings
// Configuration Bit settings using the config pragma
// SYSCLK = 80 MHz (8MHz Crystal/ FPLLIDIV * FPLLMUL / FPLLODIV)
// PBCLK = 10 MHz
/* Configuration Bit Settings */
// DEVCFG3
// USERID = No Setting

#pragma config FSRSSEL = PRIORITY_7     // SRS Select (SRS Priority 7)
#pragma config FMIIEN = OFF             // Ethernet RMII/MII Enable (RMII Enabled)
#pragma config FETHIO = OFF             // Ethernet I/O Pin Select (Alternate Ethernet I/O)
#pragma config FCANIO = OFF             // CAN I/O Pin Select (Alternate CAN I/O)
#pragma config FUSBIDIO = OFF           // USB USID Selection (Controlled by Port Function)
#pragma config FVBUSONIO = OFF          // USB VBUS ON Selection (Controlled by Port Function)

// DEVCFG2
#pragma config FPLLIDIV = DIV_2         // PLL Input Divider (2x Divider)
#pragma config FPLLMUL = MUL_20         // PLL Multiplier (20x Multiplier)
#pragma config UPLLIDIV = DIV_12        // USB PLL Input Divider (12x Divider)
#pragma config UPLLEN = OFF             // USB PLL Enable (Disabled and Bypassed)
#pragma config FPLLODIV = DIV_1         // System PLL Output Clock Divider (PLL Divide by 1)

// DEVCFG1
#pragma config FNOSC = PRIPLL           // Oscillator Selection Bits (Primary Osc w/PLL (XT+,HS+,EC+PLL))
#pragma config FSOSCEN = OFF            // Secondary Oscillator Enable (Disabled)
#pragma config IESO = OFF               // Internal/External Switch Over (Disabled)
#pragma config POSCMOD = XT             // Primary Oscillator Configuration (XT osc mode)
#pragma config OSCIOFNC = OFF           // CLKO Output Signal Active on the OSCO Pin (Disabled)
#pragma config FPBDIV = DIV_2           // Peripheral Clock Divisor (Pb_Clk is Sys_Clk/2)
#pragma config FCKSM = CSDCMD           // Clock Switching and Monitor Selection (Clock Switch Disable, FSCM Disabled)
#pragma config WDTPS = PS1              // Watchdog Timer Postscaler (1:1)
#pragma config FWDTEN = OFF             // Watchdog Timer Enable (WDT Disabled (SWDTEN Bit Controls))

// DEVCFG0
//#pragma config DEBUG = OFF             // Background Debugger Enable (Debugger is enabled)
#pragma config ICESEL = ICS_PGx2        // ICE/ICD Comm Channel Select (ICE EMUC2/EMUD2 pins shared with PGC2/PGD2)
#pragma config PWP = OFF                // Program Flash Write Protect (Disable)
#pragma config BWP = OFF                // Boot Flash Write Protect bit (Protection Disabled)
#pragma config CP = OFF                 // Code Protect (Protection Disabled)

#define SYS_FREQ		80000000L //80MHz
#define PB_CLK		40000000L //40MHz

//Define Ports
#define BLED LATGbits.LATG13
#define GLED LATEbits.LATE3
#define OLED LATEbits.LATE2
#define RLED LATAbits.LATA0

#define LED1 LATEbits.LATE0
#define LED2 LATEbits.LATE1
#define LED3 LATGbits.LATG14
#define LED4 LATGbits.LATG12

#define FRONTLED LATEbits.LATE0
#define BACKLED LATEbits.LATE1
#define BACKLED2 LATGbits.LATG14
#define SIDELED LATGbits.LATG12

#define BUZZER LATAbits.LATA9

#define CMD LATBbits.LATB12

#define MOTORF	0x1A
#define MOTORB	0x2A
#define MOTORL	0x3A
#define MOTORR	0x4A

#define ToRad(x) (x*0.01745329252)  // *pi/180
#define ToDeg(x) (x*57.2957795131)  // *180/pi

//Delay  25ns = 1
#define delay1ms 		DelayMs(1)
#define delay2ms 		DelayMs(2)
#define delay5ms 		DelayMs(5)
#define delay10ms	 	DelayMs(10)
#define delay25ms	 	DelayMs(25)
#define delay100ms	DelayMs(100)
#define delay200ms	DelayMs(200)
#define delay500ms 	DelayMs(500)
#define delay1s 		DelayMs(1000)
#define delay2s 		DelayMs(2000)
#define delay5s 		DelayMs(5000)
#define delay10s		DelayMs(10000)

#define PI  3.141592654
/* EasyCODE ) */
/* EasyCODE ( 0 
Variables */
//Logging
unsigned int 	Tcount=0;
float			LogA[500];
float			LogB[500];
//float			LogC[100];
//float			LogD[100];

//Voltages
float Vlipo=0;
unsigned short VLIPO=0;
float Current=0;
unsigned short CURRENT=0;
unsigned char N_Cells=0;

//Error
unsigned int E_count=0;
unsigned char E_bat_war=0;
unsigned char E_bat_empty=0;
unsigned char E_RF=0;
unsigned char E_CPU=0;
unsigned char E_code=0;
//LED
unsigned int BACKLED_count=0;

//Time
float Tsload=0;
float TsloadMax=0;

unsigned char High=0;
unsigned char Low=0;
signed short DataHL=0;
/* EasyCODE ( 0 */
/* EasyCODE ( 0 
matrix3f */
/* EasyCODE C */
struct matrix3f
{
        float m11,m12,m13;
        float m21,m22,m23;
        float m31,m32,m33;
};
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
vector3f */
/* EasyCODE C */
struct vector3f
{
        float x,y,z;
};
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE < */
static struct matrix3f rotationMatrix;
static float sintheta, costheta, sinomega, cosomega;
static struct vector3f accNormal, accRotated;
/* EasyCODE > */
/* EasyCODE ) */
/* EasyCODE ( 0 
UART */
//UART
float Data_Interval=0.04;
unsigned short x=0;
unsigned int Labview[60];

unsigned long LabviewOut[60];
unsigned char Data[55];
unsigned char Datapart[10];
unsigned long datahex=0;
float datafloat=0;
unsigned char TRANSMIT=0;
unsigned char sendcount=0;
unsigned char sendsize=0;
unsigned char hekcheck=0;
unsigned char dummy=0;
unsigned char grote=20;



unsigned char UART2start=0;
unsigned char UART2state=0;
unsigned char UART2counter=0;
unsigned char UART2Data=0;
unsigned char UART1state=0;
unsigned char UART1counter=0;
unsigned char UART1Data=0;
unsigned char SendSensor=0;
unsigned char Datamode=0;

//DSMX
unsigned char DSMX[20];
unsigned char DSMX_Throttle=0;
unsigned short DSMXdata=0;
float DSMXdatafloat=0;
unsigned char DSMXcounter=0;
unsigned short Channel=0;

//RF 
unsigned int RemoteCounter=0;

//Wi.232
unsigned char ACK=0;
unsigned char Register=0;
unsigned char PWRMODE=0;
unsigned char DATARATE=0;

//GPS
unsigned char GPSDATA=0;
unsigned char GPS_Data[75];
unsigned char GPSCounter=0;
unsigned char GPS_Start=0;
unsigned char GPS_Ready=0;
unsigned char GPSCRC=0;
/* EasyCODE ) */
/* EasyCODE ( 0 
System Control */
//Angles
float AngleX=0;
float AngleY=0;
float AngleZ=0;
float Altitude=0;
float Speed=0;
float declination=0.733;

float SpeedGain=4;
float AltitudeGain=4;
float LPFGain=0.1;
float AltitudeOld=0;
float AltSpeed=0;

//Control loop
unsigned char Mode=0;
unsigned char Update=0;
signed long Throttle=0;
float ThrottleStick=0;
float AngleLimit=45;
float limit=1500; //Integrator limit
float dt=0;
unsigned int loopcounter=0;
float Dfilter=50;
float DfilterS=50;

float SetpointXspeed=0;
float SetpointYspeed=0;
float SetpointZspeed=0;
signed long MotorLimit=2000;
signed long RPMXF=0;
signed long RPMXB=0;
signed long RPMYL=0;
signed long RPMYR=0;


//GPS Navi
float KrN=2;
float KiN=0.01;
float KdN=4;

float KrNS=0;
float KiNS=0;
float KdNS=0;

float LPFGainD=0.003;

float PvalueDX=0;
float IvalueDX=0;
float DvalueDX=0;
float OldErrorDX=0;
float DFilterDX=0;

float PvalueDY=0;
float IvalueDY=0;
float DvalueDY=0;
float OldErrorDY=0;
float DFilterDY=0;

float PIDDX=0;
float PIDDY=0;


//Altitude

unsigned char Max_Altitude=20;
unsigned char AltitudeHoldOn=0;
unsigned char AltitudeControlOn=0;
float SetpointA=0;
float SetpointAFilter=0;
float SetpointAspeed=0;
float ErrorA=0;
float OldErrorA=0;
float KrA=0.2;
float KiA=0;
float KdA=0;
float DFilterA=0;
float LPFGainA=0.01;
float PvalueA=0;
float IvalueA=0;
float DvalueA=0;


float KrAS=500;
float KiAS=50;
float KdAS=0;

float ErrorAS=0;
float OldErrorAS=0;
float PvalueAS=0;
float IvalueAS=0;
float IvalueAS_Old=0;
float DvalueAS=0;

float DFilterAS=0;
float LPFGainAS=0.01;

signed long PIDA=0;

//Stable Mode X
float SetpointX=0;
float ErrorXS=0;
float OldErrorXS=0;
float KrXS=2;
float KiXS=0;
float KdXS=0;
float PvalueXS=0;
float IvalueXS=0;
float DvalueXS=0;
float IDvalueXS=0;
signed long PIDXS=0;

//Acro Mode X
float ErrorX=0;
float OldErrorX=0;
float KrX=10;
float KiX=80;
float KdX=0;
float PvalueX=0;
float IvalueX=0;
float IvalueXold=0;
float DvalueX=0;
float IDvalueX=0;
signed long PIDX=0;

//Stable Mode Y
float SetpointY=0;
float ErrorYS=0;
float OldErrorYS=0;
float KrYS=4;
float KiYS=0;
float KdYS=0;
float PvalueYS=0;
float IvalueYS=0;
float DvalueYS=0;
float IDvalueYS=0;
signed long PIDYS=0;

//Acromode Y
float ErrorY=0;
float OldErrorY=0;
float KrY=10;
float KiY=80;
float KdY=0;
float PvalueY=0;
float IvalueY=0;
float DvalueY=0;
float IDvalueY=0;
signed long PIDY=0;

//stable mode Z
float SetpointZ=0;
float ErrorZS=0;
float OldErrorZS=0;
float KrZS=4;
float KiZS=0;
float KdZS=0;
float PvalueZS=0;
float IvalueZS=0;
float DvalueZS=0;
signed long PIDZS=0;
float P1=0;
float P2=0;
float P3=0;

//Acro mode Z
float ErrorZ=0;
float OldErrorZ=0;
float KrZ=15;
float KiZ=30;
float KdZ=0;
float PvalueZ=0;
float IvalueZ=0;
float IvalueZ_Old=0;
float IDvalueZ=0;
float DvalueZ=0;
signed long PIDZ=0;
/* EasyCODE ) */
/* EasyCODE ( 0 
Frequency Response */
unsigned char FreqResponse=0;

float SignalS=0;  //Injection Sine
float SignalA=0;  //Injection Sine
float SignalAS=0; //Altitude Speed
float SignalZ=0; //Speed Z
float In=0;
float Out=0;
float Error=0; 

float Frequency=0.1;
float Fstep=0.1;
float StartF=0.05;
float EndF=100;
float N=4;
float Amplitude=500;
float Time=0;

float Re_cos_In=0; //Ur
float Im_sin_In=0;	//Ur
float Re_cos_Out=0; //Y
float Im_sin_Out=0; //Y
float Re_cos_Error=0;
float Im_sin_Error=0; 
float Re_cos_Signal=0; //Y
float Im_sin_Signal=0; //Y

float Finished=0;
float Re_cos_In_send=0; //Ur
float Im_sin_In_send=0;	//Ur
float Re_cos_Out_send=0; //Y
float Im_sin_Out_send=0; //Y
float Re_cos_Error_send=0;
float Im_sin_Error_send=0; 
float Re_cos_Signal_send=0; //Signal
float Im_sin_Signal_send=0; //Signal
/* EasyCODE ) */
/* EasyCODE ( 0 
AHRS */
//#define sampleFreq	363.36f		// sample frequency in Hz
//#define betaDef		0.033f		// 2 * proportional gain IMU Without compass
//#define betaDef		0.05f		// 2 * proportional gain With compass
// Variable definitions

float betastart=1.0f;
float betasetting=0.05f;
volatile float beta = 1.0f;								// 2 * proportional gain (Kp)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame
float g0=0, g1=0, g2=0;
/* EasyCODE ) */
/* EasyCODE ( 0 
BiQuad */
//Filter
float currin   = 0.0;   // x[n]
float lastin   = 0.0;   // x[n-1]
float lastin2  = 0.0;   // x[n-2]
float currout  = 0.0;   // y[n]
float lastout  = 0.0;   // y[n-1]
float lastout2 = 0.0;   // y[n-2]

/*//2nd order 1Hz (fs=425Hz)
float a0=0.000054;
float a1=0.000108;
float a2=0.000054;
float b1=-1.985108;
float b2=0.985325;*/

//2nd order 2.5Hz (fs=425Hz)
float a0=0.000335;
float a1=0.000671;
float a2=0.000335;
float b1=-1.962378;
float b2=0.963719;

/*//2nd order 5Hz (fs=425Hz)
float a0=0.001317;
float a1=0.002634;
float a2=0.001317;
float b1=-1.923510;
float b2=0.928777;*/

/*//2nd order 10Hz (fs=425Hz)
float a0=0.005080;
float a1=0.010160;
float a2=0.005080;
float b1=-1.842483;
float b2=0.862803;
*/
/*//2nd order 5Hz (fs=31.25Hz)
float a0=0.163193;
float a1=0.326385;
float a2=0.163193;
float b1=-0.753537;
float b2=0.406308;
*/

/*//1nd order 5Hz (fs=31.25Hz)
float a0=0.335365;
float a1=0.335365;
float a2=0;
float b1=-0.329271;
float b2=0;*/

/*//2nd order 10Hz (fs=31.25Hz)
float a0=0.490831;
float a1=0.981662;
float a2=0.490831;
float b1=0.586306;
float b2=0.377018;*/
/* EasyCODE ) */
/* EasyCODE ( 0 
Motor */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned short F;
        unsigned short B;
        unsigned short L;
        unsigned short R;
        unsigned char Armed;
        
        unsigned short MaxRpm;
        unsigned short MinRpm;
}Motor;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
Gyro */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        float TEMP;
        float X;
        float Y;
        float Z;
        float Xoffset;
        float Yoffset;
        float Zoffset;
        unsigned char WHO_AM_I;
        unsigned char CONFIG;
}Gyro;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
Acc */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        float TEMP;
        float X;
        float Y;
        float Z;
        float Xoffset;
        float Yoffset;
        float Zoffset;
        
        float Zspeed;
        
        float gX;
        float gY;
        float gZ;
        float gZ_filter;
        float G;
        
        unsigned char SELF_TEST_X;
        unsigned char SELF_TEST_Y;
        unsigned char SELF_TEST_Z;
        unsigned char SELF_TEST_A;
        
        float FT_A_X;
        float FT_A_Y;
        float FT_A_Z;
        float SFT_A_X;
        float SFT_A_Y;
        float SFT_A_Z;
        float SFT_X;
        float SFT_Y;
        float SFT_Z;
        
        unsigned char CONFIG;
}Acc;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
Baro */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned short C1;
        unsigned short C2;
        unsigned short C3;
        unsigned short C4;
        unsigned short C5;
        unsigned short C6;
        unsigned int D1;
        unsigned int D2;
        
        signed int dT;
        signed int TEMP;
        signed long long OFF;
        signed long long SENS;
        signed int P;
        
        float Temperature;
        float Pressure;
        float StartPressure;
        float AltitudeOld;
        float Altitude;
        float AltitudeFilter;
        float AltitudeSpeed;
        
        float StartAltitude;
        unsigned char Timer;
        unsigned short TP;
}Baro;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
Compass */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned char ID_A;
        unsigned char ID_B;
        unsigned char ID_C;
        float X;
        float Y;
        float Z;
        float Xoffset;
        float Yoffset;
        float Zoffset;
        
        float X1, X2, X3;
        float Y1, Y2, Y3;
        float Z1, Z2, Z3;
        
        float Angle;
        
        float S_X;
        float S_Y;
        float S_Z;
}Compass;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
GPS */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned char GPGGA[75];
        unsigned char GPGLL[75];
        unsigned char GPRMC[75];
        unsigned char GPGSV[75];
        unsigned char GPGSA[75];
        unsigned char GPVTG[75];
        
        unsigned char GGACRC;
        unsigned char GLLCRC;
        unsigned char GPRMCCRC;
        unsigned char GPGSVCRC;
        unsigned char GPGSACRC;
        unsigned char GPVTGCRC;
        
        unsigned short CRC;
        
        
        float HomeLat;
        float HomeLong;
        float SetLat;
        float SetLong;
        float Target_angle;
        float Target_distance;
        
        float X_Error;
        float Y_Error;
        
        float Latitude;
        float Longitude;
        float Heading;
        float Speed;
        float Altitude;
        
        float DegLat;
        float DegLon;
        float TimeLat;
        float TimeLon;
        unsigned char N_Sat;
        unsigned char Status;
}GPS;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ( 0 
DSMX */
/* EasyCODE C */
/* EasyCODE < */
struct
/* EasyCODE > */
{
        unsigned short CH0;
        unsigned short CH1;
        unsigned short CH2;
        unsigned short CH3;
        unsigned short CH4;
        unsigned short CH5;
        unsigned short CH6;
}DSMX_Data;
/* EasyCODE E */
/* EasyCODE ) */
/* EasyCODE ) */
/* EasyCODE ( 0 
Functions */
/* EasyCODE < */
//Function declaration
void InitPort(void);
void InitINT(void);
void InitTimers(void);
void InitADC(void);
void DelayMs(unsigned int);
void cleararray(float array[], unsigned int grote );
void cleararraychar(unsigned char array[], unsigned int grote );
void cleararrayint(unsigned int array[], unsigned int grote );

float invSqrt(float x);
void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
void matrixMultiplyVector(const struct matrix3f * a, const struct vector3f * b, struct vector3f * c);

float biquad(float in);

void target(void);
/* EasyCODE > */
/* EasyCODE ) */
/* EasyCODE ) */
/* EasyCODE ( 0 
main */
/* EasyCODE F */
int main(void)
{
    SYSTEMConfigPerformance(SYS_FREQ);
    mOSCSetPBDIV(OSC_PB_DIV_2);
    delay10ms;
    InitPort();
    InitADC();
    InitINT();
    /* EasyCODE ( 0 
    I2C Reset */
    int i;
    
    LATAbits.LATA3=1;//SDA
    LATAbits.LATA2=1;//SCL
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    LATAbits.LATA3=0;//SDA
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    LATAbits.LATA3=1;//SDA
    
    int j;
    for ( j=0;j<9;j++ )
    {
        LATAbits.LATA2=0;//SCL
        for ( i=0;i<2000;i++ )
        {
            asm("NOP");
        }
        LATAbits.LATA2=1;//SCL
        for ( i=0;i<2000;i++ )
        {
            asm("NOP");
        }
    }
    LATAbits.LATA3=0;//SDA
    for ( i=0;i<2000;i++ )
    {
        asm("NOP");
    }
    LATAbits.LATA3=1;//SDA
    for ( i=0;i<1000;i++ )
    {
        asm("NOP");
    }
    /* EasyCODE ) */
    InitI2C();
    BLED=1;
    /* EasyCODE - */
    i2c_start();
    i2c_write(MPU_W);	// write 0xD2
    i2c_write(MPU_RA_WHO_AM_I );	// write register address
    
    i2c_repStart();
    i2c_write(MPU_R);	// write 0xD3
    Gyro.WHO_AM_I = i2c_read();	// Get MSB result
    i2c_nack();
    i2c_stop();
    BLED=0;
    while ( 1 )
    {
    }
}

/* EasyCODE ) */
/* EasyCODE ( 0 
Functions */
/* EasyCODE ( 0 
void InitPort(void) */
/* EasyCODE F */
void InitPort(void)
{
    DDPCONbits.JTAGEN = 0;
    
    TRISAbits.TRISA0 = 0;//RLED
    TRISAbits.TRISA1 = 0;
    TRISAbits.TRISA2 = 0; //SCL
    TRISAbits.TRISA3 = 0;//SDA
    TRISAbits.TRISA4 = 0;
    TRISAbits.TRISA5 = 0;
    TRISAbits.TRISA6 = 0;
    TRISAbits.TRISA7 = 0; 
    TRISAbits.TRISA9 = 0; //Buzzer
    TRISAbits.TRISA10 = 0; 
    TRISAbits.TRISA14 = 0; //SCL_A
    TRISAbits.TRISA15 = 0; //SDA_A
    
    TRISBbits.TRISB0 = 1;//VLIPO
    TRISBbits.TRISB1 = 1;//CURRENT 
    TRISBbits.TRISB2 = 0;
    TRISBbits.TRISB3 = 0;//DRDY
    TRISBbits.TRISB4 = 0;//CELL2
    TRISBbits.TRISB5 = 0;//CELL1
    //TRISBbits.TRISB6 = 0; //PGC 
    //TRISBbits.TRISB7 = 0;  //PGD
    TRISBbits.TRISB8 = 0;
    TRISBbits.TRISB9 = 0;
    TRISBbits.TRISB10 = 0;
    TRISBbits.TRISB11 = 0; 
    TRISBbits.TRISB12 = 0; 
    TRISBbits.TRISB13 = 0; 
    TRISBbits.TRISB14 = 0; 
    TRISBbits.TRISB15 = 0; //BT_RESET
    
    TRISCbits.TRISC1 = 0;
    TRISCbits.TRISC2 = 0;
    TRISCbits.TRISC3 = 0;
    TRISCbits.TRISC3 = 1;//SD_Data_Out
    //TRISCbits.TRISC12=0; //OSC1
    TRISCbits.TRISC13=0;
    TRISCbits.TRISC14=0;
    //TRISCbits.TRISC15=0; //OSC2
    
    TRISDbits.TRISD0=0; //SD_Data_In
    TRISDbits.TRISD1=0; 
    TRISDbits.TRISD2=1; 
    TRISDbits.TRISD3=0; 
    TRISDbits.TRISD4=0;
    TRISDbits.TRISD5=0;
    TRISDbits.TRISD6=0;
    TRISDbits.TRISD7=0;
    TRISDbits.TRISD8=0; 
    TRISDbits.TRISD9=0; //CS_SD
    TRISDbits.TRISD10=0; //SD_CLK
    TRISDbits.TRISD11=0;
    TRISDbits.TRISD12=0;
    TRISDbits.TRISD13=0;
    TRISDbits.TRISD14=1; //TXDSMX
    TRISDbits.TRISD15=0; //RXDSMX
    
    TRISEbits.TRISE0=0;//LED1
    TRISEbits.TRISE1=0;//LED2
    TRISEbits.TRISE2=0;//OLED
    TRISEbits.TRISE3=0;//GLED
    TRISEbits.TRISE4=0;//LED
    TRISEbits.TRISE5=0;
    TRISEbits.TRISE6=0;
    TRISEbits.TRISE7=0;
    TRISEbits.TRISE8=1;//DRDY
    TRISEbits.TRISE8=1;//INTA
    
    TRISFbits.TRISF0=0;
    TRISFbits.TRISF1=0;
    TRISFbits.TRISF2=1;//TX-IN
    TRISFbits.TRISF3=0;
    TRISFbits.TRISF4=1;//BTTRX
    TRISFbits.TRISF5=0;//BTRX
    TRISFbits.TRISF8=0;//RX-IN
    TRISFbits.TRISF12=1;//BTRTS
    TRISFbits.TRISF13=0;//BTCTS
    
    TRISGbits.TRISG0=0; 
    TRISGbits.TRISG1=0;
    TRISGbits.TRISG2=0;//USBD+
    TRISGbits.TRISG3=0;//USBD-
    TRISGbits.TRISG6=0;//GPSRX
    TRISGbits.TRISG7=1;//SDA/RX-IN
    TRISGbits.TRISG8=0;//SCL/TX-OUT
    TRISGbits.TRISG9=1;//GPSTX
    TRISGbits.TRISG12=0;//LED4
    TRISGbits.TRISG13=0;//BLED
    TRISGbits.TRISG14=0;//LED3
    TRISGbits.TRISG15=0;
    
    //Reset Port A
    LATAbits.LATA0=0;
    LATAbits.LATA1=0;
    LATAbits.LATA2=0;
    LATAbits.LATA3=0;
    LATAbits.LATA4=0;
    LATAbits.LATA5=0;
    LATAbits.LATA6=0;
    LATAbits.LATA7=0;
    LATAbits.LATA9=0;
    LATAbits.LATA10=0;
    LATAbits.LATA14=0;
    LATAbits.LATA15=0;
    
    //Reset Port B
    LATBbits.LATB0 = 0;
    LATBbits.LATB1 = 0;
    LATBbits.LATB2 = 0;
    LATBbits.LATB3 = 0;
    LATBbits.LATB4 = 0;
    LATBbits.LATB5 = 0;
    //LATBbits.LATB6 = 0;
    //LATBbits.LATB7 = 0;
    LATBbits.LATB8 = 0;
    LATBbits.LATB9 = 0;
    LATBbits.LATB10 = 0;
    LATBbits.LATB11 = 0;
    LATBbits.LATB12 = 0;
    LATBbits.LATB13 = 0;
    LATBbits.LATB14 = 0;
    LATBbits.LATB15 = 0;
    
    //Reset Port C
    LATCbits.LATC1=0;
    LATCbits.LATC2=0;
    LATCbits.LATC3=0;
    LATCbits.LATC4=0;
    //LATCbits.LATC12=0;
    LATCbits.LATC13=0;
    LATCbits.LATC14=0;
    //LATCbits.LATC15=0;
    
    //Reset Port D
    LATDbits.LATD0=0;
    LATDbits.LATD1=0;
    LATDbits.LATD2=0;
    LATDbits.LATD3=0;
    LATDbits.LATD4=0;
    LATDbits.LATD5=0;
    LATDbits.LATD6=0;
    LATDbits.LATD7=0;
    LATDbits.LATD8=0;
    LATDbits.LATD9=0;
    LATDbits.LATD10=0;
    LATDbits.LATD11=0;
    LATDbits.LATD12=0;
    LATDbits.LATD13=0;
    LATDbits.LATD14=0;
    LATDbits.LATD15=0;
    
    //Reset Port E
    LATEbits.LATE0=0;
    LATEbits.LATE1=0;
    LATEbits.LATE2=0;
    LATEbits.LATE3=0;
    LATEbits.LATE4=0;
    LATEbits.LATE5=0;
    LATEbits.LATE6=0;
    LATEbits.LATE7=0;
    LATEbits.LATE8=0;
    LATEbits.LATE9=0;
    
    //Reset Port F
    LATFbits.LATF0=0;
    LATFbits.LATF1=0;
    LATFbits.LATF2=0;
    LATFbits.LATF3=0;
    LATFbits.LATF4=0;
    LATFbits.LATF5=0;
    LATFbits.LATF8=0;
    LATFbits.LATF12=0;
    LATFbits.LATF13=0;
    
    //Reset Port G
    LATGbits.LATG0=0;
    LATGbits.LATG1=0;
    LATGbits.LATG2=0;
    LATGbits.LATG3=0;
    LATGbits.LATG6=0;
    LATGbits.LATG7=0;
    LATGbits.LATG8=0;
    LATGbits.LATG9=0;
    LATGbits.LATG12=0;
    LATGbits.LATG13=0;
    LATGbits.LATG14=0;
    LATGbits.LATG15=0;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void InitINT(void) */
/* EasyCODE F */
void InitINT(void)
{
    //Interrupts
    
    
    //INTCON: Interrupt Control Register
    
    //SS0: Single Vector Shadow Register Set bit(1)
    //1 =Single vector is presented with a shadow register set
    //0 =Single vector is not presented with a shadow register set
    INTCONbits.SS0=0;
    
    //MVEC: Multi Vector Configuration bit
    //1 =Interrupt controller configured for multi vectored mode
    //0 =Interrupt controller configured for single vectored mode
    INTCONbits.MVEC=1;
    
    //TPC<2:0>: Interrupt Proximity Timer Control bits
    //111 =Interrupts of group priority 7 or lower start the Interrupt Proximity timer
    //110 =Interrupts of group priority 6 or lower start the Interrupt Proximity timer
    //101 =Interrupts of group priority 5 or lower start the Interrupt Proximity timer
    //100 =Interrupts of group priority 4 or lower start the Interrupt Proximity timer
    //011 =Interrupts of group priority 3 or lower start the Interrupt Proximity timer
    //010 =Interrupts of group priority 2 or lower start the Interrupt Proximity timer
    //001 =Interrupts of group priority 1 start the Interrupt Proximity timer
    //000 =Disables Interrupt Proximity timer
    INTCONbits.TPC=0b000;
    
    //INTxEP: External Interrupt x Edge Polarity Control bit
    //1 =Rising edge
    //0 =Falling edge
    INTCONbits.INT4EP=0;
    INTCONbits.INT3EP=0;
    INTCONbits.INT2EP=0;
    INTCONbits.INT1EP=0;
    
    
    
    //IP0x<2:0>: Interrupt Priority bits
    //111 = Interrupt priority is 7
    //110 = Interrupt priority is 6
    //101 = Interrupt priority is 5
    //100 = Interrupt priority is 4
    //011 = Interrupt priority is 3
    //010 = Interrupt priority is 2
    //001 = Interrupt priority is 1
    //000 = Interrupt is disabled
    
    //IS0x<1:0>: Interrupt Subpriority bits
    //11 = Interrupt subpriority is 3
    //10 = Interrupt subpriority is 2
    //01 = Interrupt subpriority is 1
    //00 = Interrupt subpriority is 0
    IPC1bits.T1IP=0b010;  //Timer 1
    IPC1bits.T1IS=0b11;
    
    IPC7bits.U3IP=0b110;
    IPC7bits.U3IS=0b11;
    
    IPC12bits.U5IP=0b111;
    IPC12bits.U5IS=0b11;
    
    IPC12bits.U6IP=0b101;
    IPC12bits.U6IS=0b11;
    
    
    IEC0bits.T1IE=1;	//Timer Interrupt Enable
    IFS0bits.T1IF=0;
    
    IEC1bits.U3RXIE=1;
    IFS1bits.U3RXIF=0;
    
    IEC2bits.U5RXIE=1;
    IFS2bits.U5RXIF=0;
    
    IEC2bits.U6RXIE=1;
    IFS2bits.U6RXIF=0;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void InitTimers(void) */
/* EasyCODE F */
void InitTimers(void)
{
    //SIDL: Stop in Idle Mode bit
    //1 = Discontinue operation when device enters Idle mode
    //0 = Continue operation even in Idle mode
    T1CONbits.SIDL=0;
    
    //TWDIS: Asynchronous Timer Write Disable bit
    //1 = Writes to TMR1 are ignored until pending write operation completes
    //0 = Back-to-back writes are enabled (Legacy Asynchronous Timer functionality)
    T1CONbits.TWDIS=0;
    
    //TCKPS<1:0>: Timer Input Clock Prescale Select bits
    //11 = 1:256 prescale value
    //10 = 1:64 prescale value
    //01 = 1:8 prescale value
    //00 = 1:1 prescale value
    T1CONbits.TCKPS=0b01;
    
    //TCS: Timer Clock Source Select bit
    //1 = External clock from TxCKI pin
    //0 = Internal peripheral clock
    T1CONbits.TCS=0;
    
    //PR<15:0>: Period Register bits
    //These bits represent the complete 16-bit period match.
    //PR1=10000;  //500Hz
    //dt=0.002;
    PR1=12500;  //400Hz
    dt=0.0025;
    //PR1=15000;  //333Hz
    //dt=0.003;
    //PR1=20000;  //250Hz
    //dt=0.004;
    
    //ON: Timer On bit(1)
    //1 = Timer is enabled
    //0 = Timer is disabled
    T1CONbits.ON=1;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void InitADC(void) */
/* EasyCODE F */
void InitADC(void)
{
    /*SIDL: Stop in Idle Mode bit
    1 = Discontinue module operation when device enters Idle mode
    0 = Continue module operation in Idle mode*/
    AD1CON1bits.SIDL=0;
    
    /*FORM<2:0>: Data Output Format bits
    011 = Signed Fractional 16-bit (DOUT = 0000 0000 0000 0000 sddd dddd dd00 0000)
    010 = Fractional 16-bit (DOUT = 0000 0000 0000 0000 dddd dddd dd00 0000)
    001 = Signed Integer 16-bit (DOUT = 0000 0000 0000 0000 ssss sssd dddd dddd)
    000 = Integer 16-bit (DOUT = 0000 0000 0000 0000 0000 00dd dddd dddd)
    111 = Signed Fractional 32-bit (DOUT = sddd dddd dd00 0000 0000 0000 0000)
    110 = Fractional 32-bit (DOUT = dddd dddd dd00 0000 0000 0000 0000 0000)
    101 = Signed Integer 32-bit (DOUT = ssss ssss ssss ssss ssss sssd dddd dddd)
    100 = Integer 32-bit (DOUT = 0000 0000 0000 0000 0000 00dd dddd dddd)*/
    AD1CON1bits.FORM=0b000;
    
    /*SSRC<2:0>: Conversion Trigger Source Select bits
    111 = Internal counter ends sampling and starts conversion (auto convert)
    110 = Reserved
    101 = Reserved
    100 = Reserved
    011 = Reserved
    010 = Timer3 period match ends sampling and starts conversion
    001 = Active transition on INT0 pin ends sampling and starts conversion
    000 = Clearing SAMP bit ends sampling and starts conversion*/
    AD1CON1bits.SSRC=0b111;
    
    /*CLRASAM: Stop Conversion Sequence bit (when the first ADC interrupt is generated)
    1 = Stop conversions when the first ADC interrupt is generated. Hardware clears the ASAM bit when the ADC interrupt is generated.
    0 = Normal operation, buffer contents will be overwritten by the next conversion sequence*/
    AD1CON1bits.CLRASAM=0;
    
    /*ASAM: ADC Sample Auto-Start bit
    1 = Sampling begins immediately after last conversion completes; SAMP bit is automatically set
    0 = Sampling begins when SAMP bit is set*/
    AD1CON1bits.ASAM=1;
    
    /*SAMP: ADC Sample Enable bit
    1 = The ADC SHA is sampling
    0 = The ADC sample/hold amplifier is holding
    When ASAM = 0, writing ?1? to this bit starts sampling.
    When SSRC = 000, writing ?0? to this bit will end sampling and start conversion.*/
    AD1CON1bits.SAMP=0;
    
    /*DONE: Analog-to-Digital Conversion Status bit(2)
    1 = Analog-to-digital conversion is done
    0 = Analog-to-digital conversion is not done or has not started
    Clearing this bit will not affect any operation in progress.*/
    AD1CON1bits.DONE=0;
    
    /*VCFG<2:0>: Voltage Reference Configuration bits
    ADC VR+
    ADC VR-
    000		AVDD				AVSS
    001		External VREF+ pin		AVSS
    010		AVDD				External VREF- pin
    011		External VREF+ pin		External VREF- pin
    1xx		AVDD				AVSS*/
    AD1CON2bits.VCFG=0b000;
    
    /*OFFCAL: Input Offset Calibration Mode Select bit
    1 = Enable Offset Calibration mode
    VINH and VINL of the SHA are connected to VR-
    0 = Disable Offset Calibration mode
    The inputs to the SHA are controlled by AD1CHS or AD1CSSL*/
    AD1CON2bits.OFFCAL=0;
    
    
    
    /*SMPI<3:0>: Sample/Convert Sequences Per Interrupt Selection bits
    1111 = Interrupts at the completion of conversion for each 16th sample/convert sequence
    1110 = Interrupts at the completion of conversion for each 15th sample/convert sequence
    ?
    ?
    ?
    0001 = Interrupts at the completion of conversion for each 2nd sample/convert sequence
    0000 = Interrupts at the completion of conversion for each sample/convert sequence*/
    AD1CON2bits.SMPI=0b0111;
    
    /*BUFM: ADC Result Buffer Mode Select bit
    1 = Buffer configured as two 8-word buffers, ADC1BUF(7...0), ADC1BUF(15...8)
    0 = Buffer configured as one 16-word buffer ADC1BUF(15...0.)*/
    AD1CON2bits.BUFM=0;
    
    /*ALTS: Alternate Input Sample Mode Select bit
    1 = Uses MUX A input multiplexer settings for first sample, then alternates between MUX B and
    MUX A input multiplexer settings for all subsequent samples
    0 = Always use MUX A input multiplexer settings*/
    AD1CON2bits.ALTS=0;
    
    /*ADRC: ADC Conversion Clock Source bit
    1 = ADC internal RC clock
    0 = Clock derived from Peripheral Bus Clock (PBCLK)*/
    AD1CON3bits.ADRC=0;
    
    /*SAMC<4:0>: Auto-sample Time bits
    11111 = 31 TAD
    ?
    ?
    ?
    00001 = 1 TAD
    00000 = 0 TAD (Not allowed)*/
    AD1CON3bits.SAMC=0b00010;
    
    /*ADCS<7:0>: ADC Conversion Clock Select bits(1)
    11111111 = TPB ? 2 ? (ADCS<7:0> + 1) = 512 ? TPB = TAD
    ?
    ?
    ?
    00000001 = TPB ? 2 ? (ADCS<7:0> + 1) = 4 ? TPB = TAD
    00000000 = TPB ? 2 ? (ADCS<7:0> + 1) = 2 ? TPB = TAD*/
    AD1CON3bits.ADCS=0b00000001;
    
    /*CH0NB: Negative Input Select bit for MUX B
    1 = Channel 0 negative input is AN1
    0 = Channel 0 negative input is VR-*/
    AD1CHSbits.CH0NB=0;
    
    /*CH0SB<3:0>: Positive Input Select bits for MUX B
    1111 =Channel 0 positive input is AN15
    1110 =Channel 0 positive input is AN14
    1101 =Channel 0 positive input is AN13
    ?
    ?
    ?
    0001 = Channel 0 positive input is AN1
    0000 = Channel 0 positive input is AN0*/
    AD1CHSbits.CH0SB=0b0000;
    
    /*CH0NA: Negative Input Select bit for MUX A Multiplexer Setting
    1 = Channel 0 negative input is AN1
    0 = Channel 0 negative input is VR-*/
    AD1CHSbits.CH0NA=0;
    
    /*CH0SA<3:0>: Positive Input Select bits for MUX A Multiplexer Setting
    1111 = Channel 0 positive input is AN15
    1110 = Channel 0 positive input is AN14
    1101 = Channel 0 positive input is AN13
    ?
    ?
    ?
    0001 = Channel 0 positive input is AN1
    0000 = Channel 0 positive input is AN0*/
    AD1CHSbits.CH0SA=0b0000;
    
    /*PCFG<15:0>: Analog Input Pin Configuration Control bits
    1 = Analog input pin in Digital mode, port read input enabled, ADC input multiplexer input for this analog input connected to AVss
    0 = Analog input pin in Analog mode, digital port read will return as a ?1? without regard to the voltage on the pin, ADC samples pin voltage*/
    AD1PCFG=0b1111111111111100;
    
    /*CSSL<15:0>: ADC Input Pin Scan Selection bits
    1 = Select ANx for input scan
    0 = Skip ANx for input scan*/
    AD1CSSL=0b0000000000000011;
    
    /*CSCNA: Scan Input Selections for CH0+ SHA Input for MUX A Input Multiplexer Setting bit
    1 = Scan inputs
    0 = Do not scan inputs*/
    AD1CON2bits.CSCNA=1;
    /* EasyCODE - */
    /*ON: ADC Operating Mode bit(1)
    1 = ADC module is operating
    0 = ADC is off*/
    AD1CON1bits.ON=1;
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void DelayMs(unsigned int msec) */
/* EasyCODE F */
void DelayMs(unsigned int msec)
{
    unsigned int tWait, tStart; 
              
    tWait=(SYS_FREQ/2000)*msec;        //    SYS_FREQ        (80000000) 
    tStart=ReadCoreTimer();
    while ( (ReadCoreTimer()-tStart)<tWait )
    {
        ;
    }
}

/* EasyCODE ) */
/* EasyCODE ) */
/* EasyCODE ( 0 
Interrupts */
/* EasyCODE ( 0 
Timer 1 */
/* EasyCODE F */
void __ISR(_TIMER_1_VECTOR, IPL2SOFT) Timer1Handler(void)
{
    BLED=1;
    FRONTLED=1;
    /* EasyCODE - */
    Update=1;
    IFS0bits.T1IF=0;
}

/* EasyCODE ) */
/* EasyCODE ) */
/* EasyCODE ) */
